package de.hhn.mim.psb;

/**
 * 
 * Matrix zur Beschreibung von Transformationen und zur Transformation von 
 * Spaltenvektoren. Matrix 3D implementiert eine Matrix f�r homogene 3D 
 * Vektoren und ist als 4x4 Matrix realisiert.
 * Intern in Zeilen organisiert: v[zeile][spalte]
 * Methoden
 *   Multiplikation mit Vektor,
 *   Multiplikation mit Matrix
 *   Multiplikation mit Skalar
 *   Transponieren
 *   verschiedene print Funktionen (als 3x3, mit/ohne Label
 *   translation, rotationX/Y/Z
 *   
 * @author Rolf Bendl
 * 
 * 2012-05-12 Klasse erstellt.
 *
 */

public class Matrix3D
 {
  double [][] v;
  
  /**
   * Constructor, liefert Eineitsmatrix
   */
  private Matrix3D()
   {
    v = new double [4][4];
    identity();
   }
  /**
   * Constructor if mode == 0, all elements are set to 0,
   * if mode == 1, the diagonal elements are set to 1
   * @param mode
   */
  public Matrix3D(int mode)
   {
    v = new double [4][4];
    if(mode == 0) clear();
    else if(mode == 1) identity();
    else 
     {
      clear();
      System.out.println("Error in Matrix3D Constructor, illegal mode: " + 
                         mode);
     }
   }
  
  /**
   * Copy Constructor
   * @param m
   */
  public Matrix3D(Matrix3D m)
   {
    this();
    for(int i = 0; i < 4; i++)
     for(int j = 0; j < 4; j++)
       v[i][j] = m.v[i][j];    
   }
  /**
   * Copy Constructor
   * @param m Matrix als 2 dimensionaler double Array, 
   * Annahme 1. Dimension Zeilen, 2. Spalten
   */
  public Matrix3D(double [][] m)
   {
    this();
    if((m.length == 3) && (m[0].length == 3))
     {
      for(int i = 0; i < 3; i++)
       for(int j = 0; j < 3; j++)
         v[i][j] = m[i][j];    
     }
    else System.out.println("Error in Matrix3D, matrix has illegal dimension!" 
                            + " Must be 3*3");
   }
  /**
   * setzt alle Matrixelemente auf 0, Element [3][3] auf 1
   */
  public void clear()
   {
    for(int i = 0; i < 4; i++)
     for(int j = 0; j < 4; j++)
       v[i][j] = 0;
//     v[3][3] = 1.0;    
   }

  /**
   * erzeugt Einheitsmatrix
   */
  public void identity()
   {
    for(int i = 0; i < 4; i++)
     for(int j = 0; j < 4; j++)
       v[i][j] = 0;
    for(int i = 0; i < 4; i++)
       v[i][i] = 1.0;    
   }

  /**
   * Multipliziert Matrix mit Vektor v
   * v' = M * v
   * @param v der Vektor
   * @return Ergebnismatrix der Multiplikation
   */
  
  public Vect3D mult(Vect3D v)
   {
    Vect3DH   vh = new Vect3DH(v);
    double [] a  = vh.toArray();
    double [] ra = new double [4];
    
    for(int i = 0; i < 4; i++)
     {
      for(int j = 0; j < 4; j++)
       {
        ra[i] = ra[i] + this.v[i][j] * a[j];
       }
     }
    Vect3DH rh = new Vect3DH(ra);
    return(rh.toVect3D());
   }
  /**
   * Multipliziert m and die aktuelle Matrix a. Ergebnis m * a
   * Multiplikation eines Vektors v mit der Ergebnis entspricht m * (a * v)
   * @param m
   * @return Ergebnis der Matrix-Multiplikation
   */
  public Matrix3D mult(Matrix3D m)
   {
    Matrix3D r = new Matrix3D();
    double precision = 0.000001;
    
    for(int i = 0; i < 4; i++)     // alle Zeilen der Ergebnis Mat.
     {
      for(int j = 0; j < 4; j++)   //   alle Spalten der Ergebnis Mat.
       {
        double help = 0;
        for(int k = 0; k < 4; k++) //   Sum alle Elemente der Zeile i mal Elemente Spalte j
         {
          double help2 = m.v[i][k] * v[k][j];
          if(Math.abs(help2) > precision) help = help + help2;
         }
        r.v[i][j] = help;
       }
     }
    return r;
   }

  /**
   * Multiplikation mit einem Skalar
   * @param value der Skalar
   * @return Ergebnis der Multiplikation
   */
  public Matrix3D mult(double value)
   {
    Matrix3D r = new Matrix3D();
    for(int i = 0; i < 4; i++)     // alle Zeilen der Ergebnis Mat.
     {
      for(int j = 0; j < 4; j++)   //   alle Spalten der Ergebnis Mat.
       {
        double help = 0;
        r.v[i][j] = v[i][j] * value;
       }
     }
    return r;
   }
  
  /**
   * addiert Matrix m 
   * @param m die Matrix, die addiert wird
   * @return Ergebnis der Addition
   */
  public Matrix3D add(Matrix3D m)
   {
    Matrix3D r = new Matrix3D();
    for(int i = 0; i < 4; i++)     // alle Zeilen der Ergebnis Mat.
     {
      for(int j = 0; j < 4; j++)   //   alle Spalten der Ergebnis Mat.
       {
        double help = 0;
        r.v[i][j] = v[i][j] + m.v[i][j];
       }
     }
    return r;
   }

  /**
   * Transponiert die Matrix
   * @return die Transponierte
   */
  public Matrix3D transpose()
   {
    Matrix3D r = new Matrix3D();
    for(int i = 0; i < 4; i++)     // alle Zeilen der Ergebnis Mat.
     {
      for(int j = 0; j < 4; j++)   //   alle Spalten der Ergebnis Mat.
       {
        double help = 0;
        r.v[i][j] = v[j][i];
       }
     }
    return r;
   }
  
  /**
   * Gibt Matrixelemente in der Console aus
   */
  public void print()
   {
    for(int i = 0; i < 4; i++)
     {
      for(int j = 0; j < 4; j++)     
        System.out.printf("%11.6f  ", v[i][j]);
      System.out.printf("\n");
     }
   }
  
  /**
   * Gibt 3*3 Summatrix-Elemente in der Console aus
   */
  public void print3x3()
   {
    for(int i = 0; i < 3; i++)
     {
      for(int j = 0; j < 3; j++)     
        System.out.printf("%11.6f  ", v[i][j]);
      System.out.printf("\n");
     }
   }
  /**
   * Gibt Matrixelemente in der Console aus, Ausgabe beginnt mit
   * "Matrix <label>:
   */
  public void print(String label)
   {
    System.out.println("Matrix " + label + ":");
    print();
   }
  /**
   * Gibt 3*3 Summatrix-Elemente in der Console aus, Ausgabe beginnt mit
   * "Matrix <label>:
   */
  public void print3x3(String label)
   {
    System.out.println("Matrix " + label + ":");
    print3x3();
   }
  /**
   * Erzeugt Translationmatrix mit Translationsvektor t
   * @param t der Vektor
   * @return Translationsmatrix
   */
  public static Matrix3D translation(Vect3D t)
   {
    Matrix3D r = new Matrix3D();
    r.v[0][3] = t.x; 
    r.v[1][3] = t.y; 
    r.v[2][3] = t.z; 
    return r;
   }
  /**
   * Erzeugt Rotationsmatrix fuer Rotation um x-Achse um Winkel angle
   * @param angle der Winkel
   * @return die Rotationsmatrix
   */
  public static Matrix3D rotationX(double angle)
   {
    Matrix3D r = new Matrix3D();
    r.v[1][1] =  Math.cos(angle); 
    r.v[1][2] = -Math.sin(angle); 
    r.v[2][1] =  Math.sin(angle); 
    r.v[2][2] =  Math.cos(angle); 
    return r;
   }
  /**
   * Erzeugt Rotationsmatrix fuer Rotation um y-Achse um Winkel angle
   * @param angle der Winkel
   * @return die Rotationsmatrix
   */
  public static Matrix3D rotationY(double angle)
   {
    Matrix3D r = new Matrix3D();
    r.v[0][0] =  Math.cos(angle); 
    r.v[0][2] =  Math.sin(angle); 
    r.v[2][0] = -Math.sin(angle); 
    r.v[2][2] =  Math.cos(angle);    
    return r;
   }
  /**
   * Erzeugt Rotationsmatrix fuer Rotation um z-Achse um Winkel angle
   * @param angle der Winkel
   * @return die Rotationsmatrix
   */
  public static Matrix3D rotationZ(double angle)
   {
    Matrix3D r = new Matrix3D();
    r.v[0][0] =  Math.cos(angle); 
    r.v[0][1] = -Math.sin(angle); 
    r.v[1][0] =  Math.sin(angle); 
    r.v[1][1] =  Math.cos(angle); 
    return r;
   }
  
  
  
 }
